/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:59:14
 * @LastEditor: junhuizhou
 * @LastEditTime: 2020-12-30 15:35:08
 * @Description: header
 * @FilePath: \fzhtq\include\caltransmat.h
 */

#ifndef CALTRANSMAT_H_
#define CALTRANSMAT_H_

/**
 * @description: 由姿态转换矩阵计算姿态四元数
 * @algorithm: 
 * @param quat 姿态四元数
 * @param matatt 姿态矩阵
 * @return: 
 */
void Mat2Quat(double matatt[3][3], double quat[4]);

/**
 * @description: 由姿态旋转矩阵计算四元数的旋转矩阵
 * @algorithm: q0始终为正，当q0为负时,四元数全变号
 * @param qrot 四元数旋转矩阵 [即四阶实数矩阵表示四元数]
 * @param matatt 姿态旋转矩阵
 * @return: 
 */
void Mat2QuatRot(double qrot[4][4], double matatt[3][3]);

/**
 * @description: 根据姿态信息计算转换矩阵
 * @algorithm: 
 * @param matatt 姿态转换矩阵
 * @param quat 姿态四元数
 * @return: 
 */
void Quat2Mat(double quat[4], double matatt[3][3]);

/**
 * @description: 根据姿态信息计算姿态旋转矩阵
 * @algorithm: 
 * @param quat 姿态四元数
 * @param qrot 姿态旋转矩阵
 * @return: 
 */
void Quat2Qrot(double quat[4], double qrot[4][4]);

/**
 * @description: 四阶四元数姿态旋转矩阵转三阶余弦旋转矩阵
 * @algorithm: 
 * @param qrot 四阶四元数旋转矩阵
 * @param matatt 三阶余弦旋转矩阵
 * @return: 
 */
void Qrot2Mat(double qrot[4][4], double matatt[3][3]);

/**
 * @description: 由姿态四元数转姿态欧拉角
 * @algorithm: 按ZYX顺序沿自身轴转动三次的角度
 * @param quat 姿态四元数
 * @param euler 姿态欧拉角[绕x转角，绕y转角，绕z转角][-PI,PI]
 * @return: 
 */
void Quat2Euler(double quat[4], double euler[3]);

/**
 * @description: 由欧拉角转四元数
 * @algorithm: 欧拉角为321转序
 * @param euler 姿态欧拉角[绕x转角，绕y转角，绕z转角][-PI,PI]
 * @param quat 姿态四元数 
 * @return: 
 */
void Euler2Quat(double euler[3], double quat[4]);

/**
 * @description: 计算绕自身滚动轴X转动的旋转矩阵
 * @algorithm: 
 * @param matRotX 绕自身滚动轴X转动的旋转矩阵
 * @param angleRoll 绕自身滚动轴X转动的角度
 * @return: 
 */
void MatRotX(double matRotX[3][3], double angleRoll);

/**
 * @description: 计算绕自身俯仰轴Y转动的旋转矩阵
 * @algorithm: 
 * @param matRotY 绕自身俯仰轴Y转动的旋转矩阵
 * @param anglePitch 绕自身俯仰轴Y转动的角度
 * @return: 
 */
void MatRotY(double matRotY[3][3], double anglePitch);

/**
 * @description: 计算绕自身偏航轴Z转动的旋转矩阵
 * @algorithm: 
 * @param matRotZ 绕自身偏航轴Z转动的旋转矩阵
 * @param angleYaw 绕自身偏航轴Z转动的角度
 * @return: 
 */
void MatRotZ(double matRotZ[3][3], double angleYaw);

#endif
